
#include "bc_config.h"
#include "Serial.h"

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>

/* 
   ROS related info:
   -- Node subscribes to:
   "/cmd_vel", which is of type geometry_msgs/Twist

   --2 parameters have to be specified at launch time
   ->"serial_port" of type string: e.g. "/dev/ttyUSB0"
   ->"baud_rate" of type int: e.g. 9600
*/
class BaseCtrlNode
{
private:
  ros::NodeHandle nh;
  ros::Subscriber ctrl_sub;
  
  //geometry_msgs::Twist ctrl_msg; 
  // Serial for talking to Arduino
  Serial* pSerial;

public:
  BaseCtrlNode(ros::NodeHandle& n)
    :nh(n)
  {
    std::string port;
    int baud;
    n.param("serial_port", port, std::string("/dev/ttyUSB0"));
    n.param("baud_rate", baud, 9600);

    ctrl_sub = nh.subscribe("cmd_vel",10, &BaseCtrlNode::CmdCallback, this);

    pSerial = new Serial(port.c_str(), baud);
  }

  ~BaseCtrlNode()
  {
    delete pSerial;
  }

  void CmdCallback(const geometry_msgs::Twist& msg)
  {
    static uint8_t cmd[4];
    double rotZ = msg.angular.z * BASELINE;

    double vr = (msg.linear.x + rotZ) * SCALE + OFFSETf;
    double vl = (msg.linear.x + rotZ) * SCALE + OFFSETf;

    int16_t vr_t, vl_t;

    if( vr < LOW_BOUND)
      {
	ROS_WARN("VR out of range: %lf", vr);
	vr_t = LOW_BOUND;
      }
    else if (vr > HIGH_BOUND)
      {
	ROS_WARN("VR out of range: %lf", vr);
	vr_t = HIGH_BOUND;
      }
    else
      vr_t = vr;

    if( vl < LOW_BOUND)
      {
	ROS_WARN("VL out of range: %lf", vl);
	vl_t = LOW_BOUND;
      }
    else if (vl > HIGH_BOUND)
      {
	ROS_WARN("VL out of range: %lf", vl);
	vl_t = HIGH_BOUND;
      }
    else
      vl_t = vl;
    
    cmd[0] = (vl_t >> 8) & 0x3f;
    cmd[1] = (vl_t & 0x3f) | 0x40;
    cmd[2] = ((vr_t >> 8) & 0x3f ) | 0x80;
    cmd[3] = (vr_t & 0x3f) | 0xc0;

    pSerial->Write(cmd, 4);
  }
};

int main(int argc, char* argv[])
{
  ros::init(argc, argv, "base_controller");
  ros::NodeHandle n;

  BaseCtrlNode base_controller(n);

  ros::spin();
}
